#pragma once
#include <cmath>
#include "mtxlib.h"

#define PI		3.1415926535897932385f
#define PI2		6.2831853071795864769f

struct PumaPack {
	float ang1, ang2, ang3, ang4, ang5;
	float q2;

	PumaPack() {
		ang1 = ang2 = ang3 = ang4 = ang5 = 0.0f;
		q2 = 0.7f; rad = true;
	}

	bool rad;
	void ConvertToRad();
	void ConvertToDeg();
};

class Puma6C 
{
private:
	void CalculateAng1(float* pMatrix, PumaPack* res);
	void CalculateAng2(float* pMatrix, PumaPack* res);
	void CalculateAng3(float* pMatrix, PumaPack* res);
	void CalculateAng4(float* pMatrix, PumaPack* res);
	void CalculateAng5(float* pMatrix, PumaPack* res);
	void CalculateQ2(float* pMatrix, PumaPack* res);

	void DebTest(PumaPack* res);
public:
	float q1, q3, q4;

	Puma6C(float Q1 = 1.0f, float Q3 = .6f, float Q4 = .4f) {
		q1 = Q1; q3 = Q3; q4 = Q4;
	}
	bool Reverse(float* pMatrix, PumaPack* res);
	PumaPack CloseToPrev(PumaPack prev, PumaPack* res, int& index);
};